#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <math.h>
#include <iostream>
#include <unistd.h>
#include <string.h>
#include <string.h>
#include <time.h>
#include <thread>
#include <sys/socket.h>
#include <sys/time.h>
#include <sstream>
#include <vector>
#include <msgpack.hpp>
using namespace UNITREE_LEGGED_SDK;
const int PC_PORT = 8887;
const char PC_IP_ADDR[] = "localhost";
const int UDP_BUFFER_SIZE = 128;
// Globals
bool RUNNING = true; // indicating whether the main loop is running

std::string movement;
double command_number = 0.0;

class UdpReceiver
{
public:
	std::deque<std::string> msg_queue;

	/* udp receiver thread */
	void run()
	{
		/********************UDP_Receiving_Initializing********************/
		socklen_t fromlen;
		struct sockaddr_in server_recv;
		struct sockaddr_in hold_recv;

		int sock_recv = socket(AF_INET, SOCK_DGRAM, 0);

		int sock_length_recv = sizeof(server_recv);
		bzero(&server_recv, sock_length_recv);

		server_recv.sin_family = AF_INET;
		server_recv.sin_addr.s_addr = INADDR_ANY;
		server_recv.sin_port = htons(PC_PORT); // Setting port of this program
		bind(sock_recv, (struct sockaddr *)&server_recv, sock_length_recv);
		fromlen = sizeof(struct sockaddr_in);
		char buf_UDP_recv[UDP_BUFFER_SIZE]; // for holding UDP data
		/******************************************************************/
		while (RUNNING)
		{
            memset(buf_UDP_recv, 0, sizeof buf_UDP_recv);
			int datalength = recvfrom(sock_recv, buf_UDP_recv, UDP_BUFFER_SIZE, 0, (struct sockaddr *)&hold_recv, &fromlen);
            std::string str(buf_UDP_recv);
            msg_queue.push_back(buf_UDP_recv);
            while(msg_queue.size()>1){
                msg_queue.pop_front();
            }
            movement = msg_queue[0];
		}
	}
};
class Custom
{
public:
    Custom(): control(LeggedType::A1, HIGHLEVEL),udp(){control.InitCmdData(cmd);}
    void UDPRecv();
    void UDPSend();
    void RobotControl();

    Control control;
    UDP udp;
    HighCmd cmd = {0};
    HighState state = {0};
    int motiontime = 0;
};


void Custom::UDPRecv()
{
    udp.Recv();
}

void Custom::UDPSend()
{  
    udp.Send();
}

void Custom::RobotControl() 
{
	//std::cout << "     " << state.imu.quaternion[2] << std::endl;
	std::cout << state.imu.quaternion[2] << " ";
	//if(state.imu.quaternion[2] < -0.01f)
	//	exit(0);
    motiontime++;
    udp.GetRecv(state);
    float x_pos = atof(movement.c_str());
    int d = (int)x_pos;
    //std::cout << "\r          \r" << d << "  ";
    //std::cout << d << std::endl;
    // printf("%d   %f\n", motiontime, state.forwardSpeed);

    cmd.forwardSpeed = 0.0f;
    cmd.sideSpeed = 0.0f;
    cmd.rotateSpeed = 0.0f;
    cmd.bodyHeight = 0.0f;

    cmd.mode = 0;
    cmd.roll  = 0;
    cmd.pitch = 0;
    cmd.yaw = 0;
    
	float fspeed = 0.3f;

	if(motiontime < 3500)
	{
		std::cout << "justgo " << motiontime << std::endl;
		cmd.mode = 2;
		cmd.forwardSpeed = 0.5f;
		udp.SetSend(cmd);
		return;
	}
	
    if(motiontime % 3 == 0)
    {
		std::cout << "go ahead " << d << std::endl;
		cmd.mode = 2;
		cmd.forwardSpeed = fspeed;
		udp.SetSend(cmd);
		return;
	}
	
	if((280 < d && d < 350)){
         cmd.mode = 2;
         cmd.forwardSpeed = fspeed;
         std::cout << "go ahead " << d << std::endl;
    }
    if(d > 350){
         cmd.mode = 2;
         cmd.rotateSpeed = -0.1f;
         cmd.sideSpeed = -0.2f;
         std::cout << "right " << d << std::endl;
    }

    if(d > 0 && d < 280){
         cmd.mode = 2;
         cmd.rotateSpeed = 0.1f;
         cmd.sideSpeed = 0.2f;
         std::cout << "left " << d << std::endl;
    }

    if(d==0){
         cmd.mode = 2;
         cmd.forwardSpeed = fspeed; 
    }
      
	udp.SetSend(cmd);
}

int main(void) 
{
    std::cout << "Press Enter to start..." << std::endl;
    std::cin.ignore();
    printf("%ld  %ld\n", sizeof(HighCmd), sizeof(HighState) );
    printf("%ld  %ld\n", sizeof(LowCmd), sizeof(LowState) );
    UdpReceiver udp_receiver = UdpReceiver();
    std::thread udp_recv_thread(&UdpReceiver::run,&udp_receiver); // run udpRecv in a separate thread
    Custom custom;

    LoopFunc controlTask("control_loop", 0.002, 3, boost::bind(&Custom::RobotControl, &custom));
    LoopFunc udpSendTask("udp_send", 0.002, 3, boost::bind(&Custom::UDPSend, &custom));
    LoopFunc udpRecvTask("udp_recv", 0.002, 3, boost::bind(&Custom::UDPRecv, &custom));

    udpSendTask.start();
    udpRecvTask.start();
    controlTask.start();
    while(1){
        sleep(10);
    };

    return 0; 
}
